#include <buola/buola.h>
#include <buola/image/crowsizzler.h>
#include <buola/image/crowbuffer.h>
#include <buola/image/algorithm/edges.h>

namespace buola { namespace img {

void harris_corners_internal(const void *pData,bool pFloat,int pW,int pH,std::vector<std::pair<CPoint_d,float>> &pPeaks,
                    bool pLowPass=true,float pThresh=20.0,float pHarrFac=0.06,int pAreaSize=7)
{
    pPeaks.clear();

    CRowSizzler lSizzler(pW,6,6);
    int lBufW=lSizzler.GetTotalW();
    float lWThresh = pow(pThresh,4.0f)*(pAreaSize*pAreaSize);

    if (pLowPass)
        lWThresh*=pow(256.0f,4.0f);

    CRowBuffer<float> lSizBuf(lBufW,pLowPass?1:3);
    CRowBuffer<float> lLPXBuf(lBufW,pLowPass?1:0);
    CRowBuffer<float> lLPYBuf(lBufW,pLowPass?7:0);
    CRowBuffer<float> lSobBuf(lBufW,3*(pAreaSize+1));
    CRowBuffer<float> lSumBuf(lBufW,3);
    CRowBuffer<float> lValBuf(lBufW,3);
    CRowBuffer<float> lMaxBuf(lBufW,1);

    for (int y=0;y<pH;y++)
    {
        if(pFloat)
            lSizzler.Sizzle(((const float*)pData)+y*pW,lSizBuf[y]);
        else
            lSizzler.Sizzle(((const unsigned char*)pData)+y*pW,lSizBuf[y]);

        if(pLowPass)
        {
            sizzle_lowpass_x(lSizBuf[y],lLPXBuf[y],lSizzler);
            if (y==0)
                sizzle_first_lowpass_y(lLPXBuf[y],lLPYBuf[y],lLPYBuf[y+1],
                    lLPYBuf[y+2],lLPYBuf[y+3],lLPYBuf[y+4],lSizzler);
            else
                sizzle_lowpass_y(lLPXBuf[y],lLPYBuf[y],lLPYBuf[y+1],
                    lLPYBuf[y+2],lLPYBuf[y+3],lLPYBuf[y+4],lSizzler);

            if (y>=6)
                sizzle_sobel(lLPYBuf[y-2],lLPYBuf[y-1],lLPYBuf[y],
                    lSobBuf[3*y],lSobBuf[3*y+1],lSobBuf[3*y+2],lSizzler);
        }
        else
        {
            if (y>=6)
                sizzle_sobel(lSizBuf[y-2],lSizBuf[y-1],lSizBuf[y],
                             lSobBuf[3*y],lSobBuf[3*y+1],lSobBuf[3*y+2],lSizzler);
        }

        if(y>=6+pAreaSize)
        {
            sizzle_sum_window(lSobBuf[3*(y-pAreaSize)],lSobBuf[3*y],lSumBuf[0],pAreaSize,lSizzler);
            sizzle_sum_window(lSobBuf[3*(y-pAreaSize)+1],lSobBuf[3*y+1],lSumBuf[1],pAreaSize,lSizzler);
            sizzle_sum_window(lSobBuf[3*(y-pAreaSize)+2],lSobBuf[3*y+2],lSumBuf[2],pAreaSize,lSizzler);
            sizzle_harris(lSumBuf[0],lSumBuf[1],lSumBuf[2],lValBuf[y],pHarrFac,lSizzler);

            if (y>=7+pAreaSize)
                sizzle_harris_find_peak(lValBuf[y-2],lValBuf[y-1],lValBuf[y],lMaxBuf[y],
                                        lWThresh,y-(7+pAreaSize)/2,pPeaks,lSizzler);
        }
        else if(y>6)
        {
            sizzle_sum_window_first(lSobBuf[3*y],lSumBuf[0],pAreaSize,false,lSizzler);
            sizzle_sum_window_first(lSobBuf[3*y+1],lSumBuf[1],pAreaSize,false,lSizzler);
            sizzle_sum_window_first(lSobBuf[3*y+2],lSumBuf[2],pAreaSize,false,lSizzler);
            sizzle_harris(lSumBuf[0],lSumBuf[1],lSumBuf[2],lValBuf[y],pHarrFac,lSizzler);
        }
        else if(y==6)
        {
            sizzle_sum_window_first(lSobBuf[3*y],lSumBuf[0],pAreaSize,true,lSizzler);
            sizzle_sum_window_first(lSobBuf[3*y+1],lSumBuf[1],pAreaSize,true,lSizzler);
            sizzle_sum_window_first(lSobBuf[3*y+2],lSumBuf[2],pAreaSize,true,lSizzler);
        }
    }

    for(int k=0;k<pPeaks.size();k++)
        pPeaks[k].second=pThresh*sqrt(sqrt(pPeaks[k].second/lWThresh));
}

void harris_corners(const CView_gray32f &pView,std::vector<std::pair<CPoint_d,float>> &pPeaks,
                    bool pLowPass,float pThresh,float pHarrFac,int pAreaSize)
{
    harris_corners_internal(pView.RawData(),true,pView.Width(),pView.Height(),pPeaks,
                   pLowPass,pThresh,pHarrFac,pAreaSize);
}

void harris_corners(const CView_gray8 &pView,std::vector<std::pair<CPoint_d,float>> &pPeaks,
                    bool pLowPass,float pThresh,float pHarrFac,int pAreaSize)
{
    harris_corners_internal(pView.RawData(),false,pView.Width(),pView.Height(),pPeaks,
                   pLowPass,pThresh,pHarrFac,pAreaSize);
}

/*namespace cvi*/ } /*namespace buola*/ }
